from ctypes import *
import platform
import ctypes
import time
import threading
import pyvjoy
import os
import math
from ffb_cal_ori import ForceFeedbackAlgorithm
from ffb_rc import FrameData,FrameInputData,read_vehicle_status_from_rc,calculate_lateral_force,calculate_suspension_force_moment,calculate_collision_force
from MockACAPI import MockACAPI as ACAPI
from loguru import logger
import ffb_rc
import sys
import numpy as np
import matplotlib.animation as animation
import wifi_module




# 日志配置

from datetime import datetime
from pathlib import Path

# 设置带时间戳的日志文件 - 添加这几行代码
def setup_timestamped_log():

    # 移除所有现有的日志处理器 可能保留现有的日志处理器 为统一格式 设置前要移除现有
    logger.remove()
    # 创建日志目录
    logs_dir = Path(__file__).parent / "logs"
    logs_dir.mkdir(exist_ok=True)
    
    # 生成带时间戳的日志文件名
    timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
    log_file = logs_dir / f"dch_log_{timestamp}.log"

    # 添加控制台日志处理器（可选，保持默认格式也可以） level="DEBUG"输出debug以上日志   level: <8将字段宽度固定为8个字符
    logger.add(
        sys.stderr,
        level="DEBUG",
        format="<green>{time:YYYY-MM-DD HH:mm:ss}</green> | <level>{level: <8}</level>|<yellow>{function} | {name}:{line}</yellow>|  {message}"
    )

    
    # 添加文件日志处理器
    logger.add(
        str(log_file),
        level="DEBUG",
        format="{time:YYYY-MM-DD HH:mm:ss} | {level: <8}|{function} | {name}:{line}|  {message}",
        rotation="100 MB",
        retention="30 days"
    )
    # print(f"当前日志处理器数量: {len(logger._core.handlers)}")
    logger.info(f"日志已保存到: {log_file}")


# 调用函数初始化日志
setup_timestamped_log()

# 增加重试装饰器 实现断重连
from retry_decorators import retry_can, retry_network, retry_file,timing_analysis,set_decorator_config,thread_time_analysis, decorator_config

# 从共享数据模块导入
from shared_state import (
    torque_data, MAX_DATA_POINTS, G_ROLL_CNT, G_READY_ROLL_CNT, G_RATE_DIR,
    G_STEERING_RATE, G_STEERING_WHEEL_ANGLE, G_STEERING_WHEEL_ANGLE_OLD,
    G_THROTTLE, G_BRAKE, G_HAND_FORCE, G_STEERING_CAN_ID, G_STEERING_SBW_CAN_ID,
    G_THROTTLE_BRAKE_CAN_ID, run_main_flag_event,data_lock
)



fRead = None
fEnd = None
RC = False
ac_api=None
fWriteInput = None
main_thread_list = []
collision_speed = 0 

# 模块级缓存
_real_zcan = None
_mock_zcan = None
_zcan_imported = False


collision_speed = 0.0  # 记录碰撞发生时的速度
last_collision_time = -1.0  # 记录上次碰撞时间

def _init_zcan(real_can):
    global _real_zcan, _mock_zcan, _zcan_imported
    if _zcan_imported:
        return

    if real_can:
        _real_zcan = ZCAN
    else:
        from MockZCAN import MockZCAN as MockZCAN
        _mock_zcan = MockZCAN

    _zcan_imported = True

class ForceFeedbackOutput(ctypes.Structure):
    _fields_ = [
        ("tire_effect", ctypes.c_float),
        ("lateral_effect", ctypes.c_float),
        ("road_effect", ctypes.c_float),
        ("desired_torque", ctypes.c_float),
        ("friction", ctypes.c_float),
        ("damping", ctypes.c_float)
    ]

# CAN ID (使用共享数据中的定义)
INVALID_DEVICE_HANDLE = 0
INVALID_CHANNEL_HANDLE = 0

# can 结构体
from can_structures import *

from can_frames import *

class ZCAN(object):
    def __init__(self):
        if platform.system() == "Windows":
            try:
                self.__dll = windll.LoadLibrary(os.path.join(os.path.dirname(__file__), "zlgcan.dll"))
                if not self.__dll:
                    raise FileNotFoundError("LoadLibrary 返回空句柄")
            except Exception as e:
                logger.error(f"[ERROR] 加载 zlgcan.dll 失败，请确认文件是否存在。错误信息: {e}")
                self.__dll = None
        else:
            logger.error("No support now!")

    def OpenDevice(self, device_type, device_index, reserved):
        try:
            return self.__dll.ZCAN_OpenDevice(device_type, device_index, reserved)
        except:
            logger.error("Exception on OpenDevice!")
            raise

    def CloseDevice(self, device_handle):
        try:
            return self.__dll.ZCAN_CloseDevice(device_handle)
        except:
            logger.error("Exception on CloseDevice!")
            raise

    def GetDeviceInf(self, device_handle):
        try:
            info = ZCAN_DEVICE_INFO()
            ret = self.__dll.ZCAN_GetDeviceInf(device_handle, byref(info))
            return info if ret == ZCAN_STATUS_OK else None
        except:
            logger.error("Exception on ZCAN_GetDeviceInf")
            raise

    def DeviceOnLine(self, device_handle):
        try:
            return self.__dll.ZCAN_IsDeviceOnLine(device_handle)
        except:
            logger.error("Exception on ZCAN_ZCAN_IsDeviceOnLine!")
            raise

    def InitCAN(self, device_handle, can_index, init_config):
        try:
            return self.__dll.ZCAN_InitCAN(device_handle, can_index, byref(init_config))
        except:
            logger.error("Exception on ZCAN_InitCAN!")
            raise

    def StartCAN(self, chn_handle):
        try:
            logger.info("ZCAN_StartCAN 真实can 启动")
            return self.__dll.ZCAN_StartCAN(chn_handle)
        except:
            logger.error("Exception on ZCAN_StartCAN!")
            raise

    def ResetCAN(self, chn_handle):
        try:
            return self.__dll.ZCAN_ResetCAN(chn_handle)
        except:
            logger.error("Exception on ZCAN_ResetCAN!")
            raise

    def ClearBuffer(self, chn_handle):
        try:
            return self.__dll.ZCAN_ClearBuffer(chn_handle)
        except:
            logger.error("Exception on ZCAN_ClearBuffer!")
            raise

    def ReadChannelErrInfo(self, chn_handle):
        try:
            ErrInfo = ZCAN_CHANNEL_ERR_INFO()
            ret = self.__dll.ZCAN_ReadChannelErrInfo(chn_handle, byref(ErrInfo))
            return ErrInfo if ret == ZCAN_STATUS_OK else None
        except:
            logger.error("Exception on ZCAN_ReadChannelErrInfo!")
            raise

    def ReadChannelStatus(self, chn_handle):
        try:
            status = ZCAN_CHANNEL_STATUS()
            ret = self.__dll.ZCAN_ReadChannelStatus(chn_handle, byref(status))
            return status if ret == ZCAN_STATUS_OK else None
        except:
            logger.error("Exception on ZCAN_ReadChannelStatus!")
            raise

    def GetReceiveNum(self, chn_handle, can_type=ZCAN_TYPE_CAN):
        try:
            return self.__dll.ZCAN_GetReceiveNum(chn_handle, can_type)
        except:
            logger.error("Exception on ZCAN_GetReceiveNum!")
            raise

    def Transmit(self, chn_handle, std_msg, len):
        try:
            return self.__dll.ZCAN_Transmit(chn_handle, byref(std_msg), len)
        except:
            logger.error("Exception on ZCAN_Transmit!")
            raise

    def Receive(self, chn_handle, rcv_num, wait_time=c_int(-1)):
        try:
            rcv_can_msgs = (ZCAN_Receive_Data * rcv_num)()
            ret = self.__dll.ZCAN_Receive(chn_handle, byref(rcv_can_msgs), rcv_num, wait_time)
            return rcv_can_msgs, ret
        except:
            logger.error("Exception on ZCAN_Receive!")
            raise

    def TransmitFD(self, chn_handle, fd_msg, len):
        try:
            return self.__dll.ZCAN_TransmitFD(chn_handle, byref(fd_msg), len)
        except:
            logger.error("Exception on ZCAN_TransmitFD!")
            raise

    def ReceiveFD(self, chn_handle, rcv_num, wait_time=c_int(-1)):
        try:
            rcv_canfd_msgs = (ZCAN_ReceiveFD_Data * rcv_num)()
            ret = self.__dll.ZCAN_ReceiveFD(chn_handle, byref(rcv_canfd_msgs), rcv_num, wait_time)
            return rcv_canfd_msgs, ret
        except:
            logger.error("Exception on ZCAN_ReceiveFD!")
            raise
    def create_canfd_message(self, frame_data, can_id, frame_length=64):
        """
        创建CAN FD消息帧
        
        Args:
            frame_data (bytearray): 要发送的帧数据
            can_id (int): CAN ID
            frame_length (int): 帧长度，默认64
        
        Returns:
            ZCAN_TransmitFD_Data: 构造好的CAN FD消息帧
        """
        canfd_msg = ZCAN_TransmitFD_Data()
        canfd_msg.transmit_type = 1
        canfd_msg.frame.eff = 0
        canfd_msg.frame.rtr = 0
        canfd_msg.frame.brs = 0
        canfd_msg.frame.len = frame_length
        canfd_msg.frame.can_id = can_id

        for i in range(len(frame_data)):
            canfd_msg.frame.data[i] = frame_data[i]
        
        return canfd_msg
    def GetIProperty(self, device_handle):
        try:
            self.__dll.GetIProperty.restype = POINTER(IProperty)
            return self.__dll.GetIProperty(device_handle)
        except:
            logger.error("Exception on ZCAN_GetIProperty!")
            raise

    def SetValue(self, iproperty, path, value):
        try:
            func = CFUNCTYPE(c_uint, c_char_p, c_char_p)(iproperty.contents.SetValue)
            return func(c_char_p(path.encode("utf-8")), c_char_p(value.encode("utf-8")))
        except:
            logger.error("Exception on IProperty SetValue")
            raise

    def GetValue(self, iproperty, path):
        try:
            func = CFUNCTYPE(c_char_p, c_char_p)(iproperty.contents.GetValue)
            return func(c_char_p(path.encode))
        except:
            logger.error("Exception on IProperty GetValue")
            raise

    def ReleaseIProperty(self, iproperty):
        try:
            return self.__dll.ReleaseIProperty(iproperty)
        except:
            logger.error("Exception on ZCAN_ReleaseIProperty!")
            raise



@retry_can
def can_start(zcanlib, device_handle, chn):
    ip = zcanlib.GetIProperty(device_handle)
    ret = zcanlib.SetValue(ip, str(chn) + "/clock", "60000000")
    if ret != ZCAN_STATUS_OK:
        logger.error("Set CH%d CANFD clock failed!" % (chn))
    ret = zcanlib.SetValue(ip, str(chn) + "/canfd_standard", "0")
    if ret != ZCAN_STATUS_OK:
        logger.error("Set CH%d CANFD standard failed!" % (chn))
    ret = zcanlib.SetValue(ip, str(chn) + "/initenal_resistance", "1")
    if ret != ZCAN_STATUS_OK:
        logger.error("Open CH%d resistance failed!" % (chn))
    zcanlib.ReleaseIProperty(ip)

    chn_init_cfg = ZCAN_CHANNEL_INIT_CONFIG()
    chn_init_cfg.can_type = ZCAN_TYPE_CANFD
    chn_init_cfg.config.canfd.abit_timing = 104286
    chn_init_cfg.config.canfd.dbit_timing = 4260362
    chn_init_cfg.config.canfd.mode = 0
    chn_handle = zcanlib.InitCAN(device_handle, chn, chn_init_cfg)
    if chn_handle is None:
        return None
    zcanlib.StartCAN(chn_handle)
    return chn_handle

def record_torque_data(**kwargs):
    global torque_data
    with data_lock:
        # 直接添加数据，defaultdict 会自动处理新键
        for key, value in kwargs.items():
            torque_data.append_data(key,value)

        # 确保转向相关数据也被记录
        steering_fields = {
            'steering_angle': G_STEERING_WHEEL_ANGLE,
            'steering_rate': G_STEERING_RATE,
            'rate_dir': G_RATE_DIR
        }
        
        for field, default_value in steering_fields.items():
            if field not in kwargs:
                torque_data.append_data(field,default_value)

def read_vehicle_status(ac_api):
    if ac_api:
        roll = ac_api.AC_GetRoll()
        pitch = ac_api.AC_GetPitch()
        speed = ac_api.AC_GetSpeedKmh()
        wheel_slip = ac_api.AC_GetWheelSlip()
        acc_g = ac_api.AC_GetAccG()
        suspension_travel = ac_api.AC_GetSuspensionTravel()
        local_angular_vel = ac_api.AC_GetLocalAngularVel()
        return roll, pitch, speed, wheel_slip, acc_g, suspension_travel, local_angular_vel
    else:
        return 0.0, 0.0, 0.0, None, None, None, None

@thread_time_analysis
def send_messages(chn_handle, ac_api, zcanlib):
    global collision_speed,G_BRAKE,G_THROTTLE
    transmit_canfd_num = 7
    canfd_msgs = (ZCAN_TransmitFD_Data * transmit_canfd_num)()

    # can_ids = [0x0C3, 0x0C3, 0x0C3, 0x29F, 0x29F, 0x29F, 0x341]
    can_ids = [0x0C3, 0x29F, 0x29F, 0x29F, 0x341]

    for i, can_id in enumerate(can_ids):
        msg = canfd_msgs[i]
        msg.transmit_type = 1
        msg.frame.eff = 0
        msg.frame.rtr = 0
        msg.frame.brs = 0
        msg.frame.len = 8
        msg.frame.can_id = can_id

    try:
        cnt = 0
        execution_count = 0
        while run_main_flag_event.is_set():
            execution_count += 1
            loop_start_time = time.perf_counter()
            logger.info(f"[发送逻辑]] 发送开始")
            roll = 0.0
            pitch = 0.0
            speed = 0.0

            # 初始化变量，避免在if分支中未定义
            friction = 0.0
            damping = 0.0
            lateral_effect = 0.0
            suspension_effect = 0.0
            total_torque = 0.0
            scale_torque = 0.0
            collision = 0.0

            if RC:
                roll, pitch, speed,suspensionData,tyreData,timeSinceLastCollision,acceleration= read_vehicle_status_from_rc()
                speed=speed*3.6
            else:
                roll, pitch, speed, wheel_slip, acc_g, suspension_travel, local_angular_vel = read_vehicle_status(ac_api)   

            roll_pitch_frame_data, roll_raw_real, roll_add_real, pitch_real = encode_roll_pitch_frame('U7', G_THROTTLE,
                                                            G_BRAKE, roll,
                                                            pitch, speed,
                                                            G_STEERING_WHEEL_ANGLE,
                                                            G_STEERING_WHEEL_ANGLE_OLD,
                                                            G_STEERING_RATE)
            
            pitch=0
            roll=0
            for i in range(len(roll_pitch_frame_data)):
                for msg in canfd_msgs[:3]:
                    msg.frame.data[i] = roll_pitch_frame_data[i]

            cnt += 1

            if cnt == 5:
                cnt = 0
                switch_vr_frame_data = encode_switch_vr_frame(0x2)
                
                for i in range(len(switch_vr_frame_data)):
                    for msg in canfd_msgs[3:-1]:
                        msg.frame.data[i] = switch_vr_frame_data[i]

            ready_frame = encode_sbw_ready_frame()
            for i in range(len(ready_frame)):
                canfd_msgs[6].frame.data[i] = ready_frame[i]
                
            if chn_handle is not None:
                roll_pitch_steer_ready_ret = zcanlib.TransmitFD(chn_handle, canfd_msgs, transmit_canfd_num)
                logger.info(f"roll,pitch：{roll_pitch_steer_ready_ret}")

            ffb=ForceFeedbackAlgorithm()
            desired_torque=ffb.get_tanh_torque(speed,G_STEERING_WHEEL_ANGLE)
            friction, damping = ffb.get_friction(ctypes.c_float(G_STEERING_WHEEL_ANGLE),
                                    ctypes.c_float(G_STEERING_RATE))

            if RC:
                if timeSinceLastCollision<0.08:
                    collision_speed=speed
                logger.info(f"timeSinceLastCollision:{timeSinceLastCollision:.4f}")
                collision=calculate_collision_force(collision_speed, timeSinceLastCollision)
                # collision_frame_data = encode_sbw_ffb_frame(collision)

                # collision_canfd_msgs = zcanlib.create_canfd_message(collision_frame_data, 0x57, 64)

                # if chn_handle is not None:
                #     ret_collison = zcanlib.TransmitFD(chn_handle, collision_canfd_msgs, 1)
                #     logger.debug("collison", ret_collison)
                lateral_effect = 0.0
                for i in range(4):  # 4个轮胎
                    lateral_effect += calculate_lateral_force(tyreData[i],speed,acceleration,timeSinceLastCollision)
            
                
                suspension_effect = calculate_suspension_force_moment(suspensionData)
                total_torque = desired_torque-damping 
                scale_torque=total_torque*0.05*0.001*-1
            else :

                lateral_effect = ffb.get_lateral_effect(
                    speed,
                    acc_g,
                    local_angular_vel,
                    wheel_slip.slip
                )
                suspension_effect=ffb.get_suspension_effect(speed, suspension_travel)

                total_torque = desired_torque - damping - friction+lateral_effect+suspension_effect
                logger.info(f"total_torque: {total_torque:.2f}")
                scale_torque = total_torque * 0.001 * 0.05 * -1

            


            

            logger.info(f"desired_torque: {desired_torque:.2f}, damping: {damping:.2f}, friction: {friction:.2f}, total: {total_torque:.2f}")

            
            logger.info(f"scale_torque: {scale_torque:.6f}")

            hand_torque=scale_torque-G_HAND_FORCE*0.0714*0.001
            hand_torque=total_torque

            record_torque_data(
                desired_torque=desired_torque,
                damping=damping,
                friction=friction,
                total_torque=total_torque,
                scale_torque=scale_torque,
                lateral_effect=lateral_effect,
                suspension_effect=suspension_effect,
                hand_force=G_HAND_FORCE,
                hand_torque=hand_torque
            )

            ffb_frame_data = encode_sbw_ffb_frame(scale_torque)

            steer_canfd_msgs = zcanlib.create_canfd_message(ffb_frame_data, 0x57, 64)

            if chn_handle is not None:
                ret_steer = zcanlib.TransmitFD(chn_handle, steer_canfd_msgs, 1)
                logger.debug("steer", ret_steer)

            
            loop_end_time = time.perf_counter()
            # 记录时序信息
            if decorator_config.thread_time_analysis_enabled and execution_count % 50 == 0:
                execution_duration = loop_end_time - loop_start_time
                logger.debug(f"[线程时序] send_messages "
                           f"第{execution_count}次执行 "
                           f"开始:{loop_start_time:.6f}s "
                           f"结束:{loop_end_time:.6f}s "
                           f"执行时间:{execution_duration*1000:.2f}ms")

            time.sleep(0.04)

    except KeyboardInterrupt:
        logger.error("Send interrupted by user")
    except Exception as e:
        logger.error(f"Error occurred: {e}")
    finally:
        logger.info("Send thread exiting")

@thread_time_analysis
def receive_messages(chn_handle, zcanlib):
    global G_STEERING_WHEEL_ANGLE_OLD, G_STEERING_WHEEL_ANGLE, G_STEERING_RATE, G_RATE_DIR,G_HAND_FORCE,G_THROTTLE,G_BRAKE
    rc_inputdata = FrameInputData()#由于发送频率过快 单独创建数据结构 steering会冲掉 throttle和brake 放在公共部分
    execution_count = 0
    try:
        while run_main_flag_event.is_set():
            execution_count += 1
            loop_start_time = time.perf_counter()
            logger.info(f"[接收逻辑] 开始第{execution_count}次接收")

            rcv_canfd_num = zcanlib.GetReceiveNum(chn_handle, ZCAN_TYPE_CANFD)
            # with data_lock:
            #     torque_data['steering_angle'].append(G_STEERING_WHEEL_ANGLE)
            #     torque_data['steering_rate'].append(G_STEERING_RATE)
            #     torque_data['rate_dir'].append(G_RATE_DIR)

            #     for key in [ 'steering_angle', 'steering_rate', 'rate_dir']:
            #         if len(torque_data[key]) > MAX_DATA_POINTS:
            #             try:
            #                 torque_data[key].pop(0)
            #             except IndexError:
            #                 pass

            if rcv_canfd_num:
                rcv_canfd_msgs, rcv_canfd_num = zcanlib.ReceiveFD(chn_handle, rcv_canfd_num, 100)
                for i in range(rcv_canfd_num):
                    frame = rcv_canfd_msgs[i].frame
                    can_id = frame.can_id
                    data = frame.data

                    if can_id == G_STEERING_SBW_CAN_ID:
                        G_STEERING_WHEEL_ANGLE_OLD = G_STEERING_WHEEL_ANGLE

                        steering_angle, G_STEERING_RATE = decode_steering_wheel_angle_frame(can_id, data)
                        steering_wheel_factor = 1.6
                        G_STEERING_WHEEL_ANGLE = steering_angle * steering_wheel_factor
                        logger.debug(f"G_STEERING_WHEEL_ANGLE_OLD: {G_STEERING_WHEEL_ANGLE_OLD}")
                        logger.debug(f"G_STEERING_WHEEL_ANGLE: {G_STEERING_WHEEL_ANGLE}")
                        delta_angle = G_STEERING_WHEEL_ANGLE_OLD - G_STEERING_WHEEL_ANGLE
                        G_RATE_DIR = 1 if delta_angle > 0 else -1
                        logger.debug(f"G_RATE_DIR: {G_RATE_DIR}")

                        G_STEERING_RATE = G_STEERING_RATE * G_RATE_DIR
                        G_HAND_FORCE = decode_hand_force_data(data)

                        if RC:
                            
                            rc_inputdata.steer = steering_angle / 360
                            logger.debug(f"rc_inputdata.steer: {rc_inputdata.steer}") 
                            fWriteInput(rc_inputdata)
                        else:
                            try:
                                pyvjoy.VJoyDevice(1).set_axis(pyvjoy.HID_USAGE_Z,
                                            int((G_STEERING_WHEEL_ANGLE + 720) / 1440 * 32767))
                            except Exception as e:
                                logger.error(f"Failed to set vJoy axis: {e}")
                            logger.debug(f"steering_angle: {steering_angle}")

                    if can_id == G_THROTTLE_BRAKE_CAN_ID:
                        throttle, brake = decode_throttle_brake_frame(data)
                        logger.debug(f"throttle:{throttle}    brake:{brake}\n")
                                                # 修复：更新全局油门和刹车变量
                        G_THROTTLE = throttle
                        G_BRAKE = brake
                        if RC:
                            
                            rc_inputdata.throttle = throttle*2/ 100
                            rc_inputdata.brake = brake*2.5/ 100
                            
                            fWriteInput(rc_inputdata)
                            logger.debug(f"rc_inputdata.throttle:{rc_inputdata.throttle}\n")
                            logger.debug(f"rc_inputdata.brake:{rc_inputdata.brake}\n")
                            
                        else:
                            pyvjoy.VJoyDevice(1).set_axis(pyvjoy.HID_USAGE_Y, int(brake * 3 / 100 * 32767))
                            pyvjoy.VJoyDevice(1).set_axis(pyvjoy.HID_USAGE_X, int(throttle / 100 * 32767))
            else:
                pass
            loop_end_time = time.perf_counter()
            if decorator_config.thread_time_analysis_enabled and execution_count % 50 == 0:
                execution_duration = loop_end_time - loop_start_time
                logger.debug(f"[线程时序] receive_messages "
                            f"第{execution_count}次执行 "
                            f"开始:{loop_start_time:.6f}s "
                            f"结束:{loop_end_time:.6f}s "
                            f"执行时间:{execution_duration*1000:.2f}ms")
    except KeyboardInterrupt:
        logger.error("Rec interrupted by user")

def update_g_vars(angle, rate, direction):
    global G_STEERING_WHEEL_ANGLE_OLD, G_STEERING_WHEEL_ANGLE, G_STEERING_RATE, G_RATE_DIR

    G_STEERING_WHEEL_ANGLE_OLD = G_STEERING_WHEEL_ANGLE
    G_STEERING_WHEEL_ANGLE = angle
    G_STEERING_RATE = rate
    G_RATE_DIR = direction

    torque_data['steering_angle'].append(G_STEERING_WHEEL_ANGLE)
    torque_data['steering_rate'].append(G_STEERING_RATE)
    torque_data['rate_dir'].append(G_RATE_DIR)

    for key in [ 'steering_angle', 'steering_rate', 'rate_dir']:
        if len(torque_data[key]) > MAX_DATA_POINTS:
            torque_data[key].pop(0)
@retry_can
def initialize_can(config, window=None):
    use_real_can = config.get('USE_REAL_CAN', False)
    _init_zcan(use_real_can)
    ZCAN = _mock_zcan if not use_real_can else _real_zcan

    try :
        zcanlib = ZCAN()
        # 使用包装器包装 zcanlib 实例
        if use_real_can:
            from zcan_wrapper import ZCANWrapper
            zcanlib = ZCANWrapper(zcanlib)
    except Exception as e:
        logger.error("Error initializing ZCAN:", e)
        exit(1)
        
    # 如果有窗口对象，设置zcanlib
    if hasattr(window, 'set_zcanlib'):
        window.set_zcanlib(zcanlib)

    if config['USE_REAL_CAN']:
        handle = zcanlib.OpenDevice(ZCAN_USBCANFD_MINI, 0, 0)
        if handle == INVALID_DEVICE_HANDLE:
            logger.error("Open Device failed!")
            exit(0)
        logger.debug(f"device handle: {handle}")

        info = zcanlib.GetDeviceInf(handle)
        logger.debug(f"Device Info:\n{info}")

        chn_handle = can_start(zcanlib, handle, 0)
        logger.debug(f"channel handle: {chn_handle}")
    else:
        handle = "simulated_device"
        chn_handle = "simulated_channel_0"
        zcanlib.StartCAN(chn_handle)
        logger.info("+++++++++++++++++++++++++Running in simulation mode without ZCAN device.++++++++++++++++")
        zcanlib.on_steering_update = lambda a, r, d: update_g_vars(a, r, d)

    return zcanlib, chn_handle,handle

def start_main_process(config, window=None):
    global main_thread_list,fRead,fWriteInput,fEnd,ac_api,RC

    main_thread_list.clear()

    if config['USE_REAL_AC']:
        if config['USE_RC']:
            ffb_rc.init_game_api()
            fRead = ffb_rc.get_fRead()
            fEnd = ffb_rc.get_fEnd()
            fWriteInput=ffb_rc.get_fWriteInput()
            RC = True
        else :
            RC= False
            ac_api = windll.LoadLibrary(os.path.join(os.path.dirname(__file__), "4_ac_api.dll"))

            ac_api.AC_GetRoll.restype = ctypes.c_float
            ac_api.AC_GetPitch.restype = ctypes.c_float

            ac_api.AC_StartUpdate.restype = ctypes.c_int
            ac_api.AC_GetSpeedKmh.restype = ctypes.c_float
            ac_api.AC_GetSteerAngle.restype = ctypes.c_float
            ac_api.AC_FFB_GetForceFeedbackWithEffects.restype = ForceFeedbackOutput

            class WheelSlip(ctypes.Structure):
                _fields_ = [("slip" , ctypes.c_float*4)]
            ac_api.AC_GetWheelSlip.restype = WheelSlip

            class AccG(ctypes.Structure):
                _fields_ = [("accg", ctypes.c_float * 3)]
            ac_api.AC_GetAccG.restype = AccG

            class SuspensionTravel(ctypes.Structure):
                _fields_ = [("st", ctypes.c_float * 4)]
            ac_api.AC_GetSuspensionTravel.restype = SuspensionTravel

            class LocalAngularVel(ctypes.Structure):
                _fields_ = [("VehAngVel", ctypes.c_float * 3)]
            ac_api.AC_GetLocalAngularVel.restype = LocalAngularVel

            if ac_api.AC_StartUpdate() != 0:
                logger.error("请先打开神力科莎")
                exit(1)
    else:
        logger.info("Using mock ac_api.")
        ac_api = ACAPI()

    if config['USE_WIFI']:
        send_broadcast_thread = threading.Thread(
            target=wifi_module.wifi_send_broadcast_messages,
            name="WiFi_Broadcast_Sender"
        )
        sender_heartbeat_thread = threading.Thread(
            target=wifi_module.wifi_send_heartbeat_messages,
            name="WiFi_Heartbeat_Sender"
        )
        sender_thread = threading.Thread(
            target=wifi_module.wifi_send_messages, 
            args=(ac_api,),
            name="WiFi_Data_Sender"
        )
        receiver_thread = threading.Thread(
            target=wifi_module.wifi_receive_messages,
            name="WiFi_Data_Receiver"
        )
        heartbeat_monitor_thread = threading.Thread(
            target=wifi_module.wifi_heartbeat_monitor,
            name="WiFi_Heartbeat_Monitor"
        )
        running_threads = [
            receiver_thread,
            send_broadcast_thread,
            heartbeat_monitor_thread,
            sender_heartbeat_thread,
            sender_thread
        ]
    else:
        zcanlib, chn_handle,handle = initialize_can(config, window)
        
        # 如果有窗口对象，设置chn_handle
        if window is not None and hasattr(window, 'chn_handle'):
            window.chn_handle = chn_handle
            
        sender_thread = threading.Thread(
            target=send_messages, 
            args=(chn_handle, ac_api, zcanlib),
            name="CAN_Data_Sender"
        )
        receiver_thread = threading.Thread(
            target=receive_messages, 
            args=(chn_handle, zcanlib),
            name="CAN_Data_Receiver"
        )
        running_threads = [sender_thread, receiver_thread]
        
    for t in running_threads:
        t.daemon = True
        t.start()

    def monitor_threads():
        try:
            for t in running_threads:
                t.join()
        except KeyboardInterrupt:
            logger.error("Main process interrupted by user.")
        finally:
            if not config['USE_WIFI']:
                zcanlib.ResetCAN(chn_handle)
                zcanlib.CloseDevice(handle)
    
    monitor_thread = threading.Thread(target=monitor_threads,name="Main_Process_Monitor")
    # monitor_thread.daemon = True # 确保主进程在子线程结束后结束
    monitor_thread.start()
    # 添加到返回值中，以便调用者可以管理这个线程
    return running_threads, monitor_thread
# 在程序启动时配置
# def setup_application(config):
#     """设置应用程序"""
#     # 配置装饰器
#     configure_application_decorators(config)
    
#     # 其他初始化代码...

def configure_application_decorators(config):
    """配置应用程序装饰器"""
    decorator_settings = {
        'performance_monitoring_enabled': config.get('ENABLE_PERFORMANCE_MONITORING', True),
        'debug_logging_enabled': config.get('ENABLE_DEBUG_LOGGING', True),
        'call_time_logging_enabled': config.get('ENABLE_CALL_TIME_LOGGING', True),
        'thread_time_analysis_enabled': config.get('ENABLE_THREAD_TIME_ANALYSIS', True),  # 新增配置
    }
    
    set_decorator_config(**decorator_settings)
    logger.info("装饰器配置完成")

if __name__ == "__main__":

    config = {
    'USE_WIFI': False,
    'USE_REAL_CAN': False,
    'USE_REAL_AC': False,
    'USE_RC': False,
    'ENABLE_PERFORMANCE_MONITORING': True,
    'ENABLE_DEBUG_LOGGING': True,
    'ENABLE_CALL_TIME_LOGGING': True,
    'ENABLE_THREAD_TIME_ANALYSIS': True,  # 新增配置项
    }
    configure_application_decorators(config)
    config_ready_event = threading.Event()

    no_gui = "--no" in sys.argv or "-n" in sys.argv
    if no_gui:
        logger.info("Running without GUI...")
        start_main_process(config)
    else:
        from PyQt5.QtWidgets import QApplication

        from gui import RealTimePlotWindow
        
        app = QApplication(sys.argv)
        window = RealTimePlotWindow(config)
        window.show()
        
        sys.exit(app.exec())